#include "../include/control.hpp"

//规划的s、v、a转化为a用于控制
void Control::TrajectoryToAccelaration(){
    float final_acc = 0.f;
    //采用双P控制，将距离和速度映射到加速度上
    float vel_diff_ms = (input.expect_velocity_kph - input.vehicle_velocity_kph)/3.6f;
    float dis_diff_ratio = input.expect_distance/tool.DivideProtect(tool.Satuation(input.vehicle_velocity_kph * 0.5f, 10.f, 50.f));
    float vel_to_acc = tool.Satuation(vel_diff_ms*ACC_C_GainDiffVel, -2.f, 2.f);
    float dis_to_acc = 0.f;
    //正负距离采用不同的P控制，通常负值采用更大的P
    if(dis_diff_ratio > 0){
        dis_to_acc = tool.Satuation(dis_diff_ratio*ACC_C_GainDiffPos, -2.f, 2.f);
    }
    else{
        dis_to_acc = tool.Satuation(dis_diff_ratio*ACC_C_GainDiffPosNegative, -2.f, 2.f);
    }
    final_acc = tool.Satuation(dis_to_acc + vel_to_acc, -3.f, 2.f) + input.expect_accelaration;
    
    internal_state.expect_accelaration = final_acc;
}

//自车速度计算加速度
void Control::VelocitySolveAccelaration(){
    float velocity_kph_delay500ms = tool.Delay(1, input.vehicle_velocity_kph, 50);
    float accelaration = (input.vehicle_velocity_kph - velocity_kph_delay500ms)*2.f/3.6f;
    float accelaration_filter = tool.ButterworthFilter(1, accelaration, 0.4, 100);
    internal_state.ego_accelaration = accelaration_filter;
}

//扭矩前馈计算
void Control::ThrottleFeedForward(){
    float pitch_to_acc = sin(input.pitch_angle/57.3f)*9.8*0.7;
    float feed_acc = internal_state.expect_accelaration + pitch_to_acc;
    
    //根据标定表车速查出不同车速下加速度对应的扭矩
    TableBP vel_thr_table;
    for(auto it = ACC_C_TableVelThrAcc.begin(); it != ACC_C_TableVelThrAcc.end(); it++){
        float tmp_throttle = tool.LookUpTable(feed_acc, it->second);
        vel_thr_table.BP.push_back(it->first);
        vel_thr_table.Table.push_back(tmp_throttle);
    }

    float throttle_feedforward = tool.LookUpTable(input.vehicle_velocity_kph, vel_thr_table);
    internal_state.throttle_feedforward = throttle_feedforward;
}

//扭矩反馈计算
void Control::ThrottleFeedBack(){
    float integral_value = 0;
    bool feed_torque_push_flag = 0;
    static int count = 0;

    //积分器的积分输入计算
    //如果系统制动，那么在制动后的0.5秒，积分量均为0
    if(internal_state.brake_enable_flag == 1){
        feed_torque_push_flag = 1;
        count = 50;
    }
    else if(internal_state.brake_enable_flag == 0 && count == 0){
        feed_torque_push_flag = 0;
    }
    else{
        count = std::max(count - 1,0);
    }
    //当flag == 1时，积分量 = 0，其余正常输入
    if(feed_torque_push_flag == 1){
        integral_value = 0;
    }
    else{
        integral_value = (internal_state.expect_accelaration - internal_state.ego_accelaration)*ACC_C_PIDI;
    }
    //扭矩输出关闭flag
    bool torque_off_flag = 0;
    if(internal_state.brake_enable_flag == 1 || input.control_enable == 0 || input.stop_flag == 1){
        torque_off_flag = 1;
    }
    else{
        torque_off_flag = 0;
    }
    //扭矩积分重置flag，做了一个保护，防止底盘没数据时，车速为0，导致积分向上增加
    bool torque_reset_flag = 0;
    if(torque_off_flag == 1 || input.vehicle_velocity_kph < 1){
        torque_reset_flag = 1;
    }
    else{
        torque_reset_flag = 0;
    }
    //前馈结果
    float feed_forward_final = 0;
    if(torque_off_flag == 1){
        feed_forward_final = 0;
    }
    else{
        feed_forward_final = internal_state.throttle_feedforward;
    }
    float feedback_integral = tool.Integrate(1, integral_value, -1*feed_forward_final, ACC_C_PIDIUpLimit, 100, torque_reset_flag);
    float final_throttle = feedback_integral + feed_forward_final;
    internal_state.throttle_final = final_throttle;
}

//刹车控制
void Control::BrakeControl(){
    float brake_value = 0;
    bool brake_flag = 0;
    float slip_acceleration = tool.LookUpTable(input.expect_velocity_kph, ACC_C_TableSlipDec);
    brake_value = tool.LookUpTable(internal_state.expect_accelaration, ACC_C_TableDecBrakePadel);
    //决策是否输出刹车->brake_flag
    if(internal_state.expect_accelaration < 1.2*slip_acceleration){
        brake_flag = 1;
    }
    if(internal_state.expect_accelaration > slip_acceleration && internal_state.expect_accelaration > -0.1){
        brake_flag = 0;
    }
    if(brake_flag == 0){
        brake_value = 0;
    }
    if(input.stop_flag == 1){
        brake_value = ACC_C_StopBrakePressure;
    }
    internal_state.brake_value = brake_value;
    internal_state.brake_enable_flag = brake_flag;
}

//由车身姿态引起的方向盘控制量
void Control::AngleFeedback(){
    static float angle_pid_p = 0.f;
    static int count = 0;
    static std::vector<float> wheel_angle_history(10, 0);
    count++;
    //每一行0.1s记录一次前轮转角，共记录10次
    if(count == 10){
        float tmp = angle_pid_p/57.3/LCC_C_RatioGear;
        wheel_angle_history.insert(wheel_angle_history.begin(),tmp);
        if(wheel_angle_history.size() > 10){
            wheel_angle_history.pop_back();
        }
        count = 0;
    }
    /*根据记录的前轮转角进行车身姿态推算,高速高延时有用，低速低延时没啥用
    通过运动学推算，参考纯跟踪，公式如下
        sin(alpha) = ld/2R
        R = L/tan(theta)
        alpha = asin(ld*tan(theta)/2/DisWheelbase)*/
    float move_s_100ms = 0.1*input.vehicle_velocity_kph/3.6;
    float c0_predict = input.c0;
    float c1_predict = input.c1;
    int predict_number = LCC_C_TimeSystemDelay/100;
    for(int i = 0; i < predict_number; i++){
        float tmp = move_s_100ms*tan(wheel_angle_history[i])/2/LCC_C_DisWheelbase;
        float alpha = asin(tool.Satuation(tmp,-0.9,0.9));
        c0_predict = c0_predict - move_s_100ms*sin(alpha) + move_s_100ms*sin(c1_predict);
        c1_predict = c1_predict - alpha;
    }
    //计算车辆当前位置与预瞄点形成的夹角
    float predict_dis = tool.Satuation(input.vehicle_velocity_kph/3.6*LCC_C_TimePredictSecond,6,50);
    float predict_point_angle = atan((c0_predict + c1_predict*predict_dis)/predict_dis);
    angle_pid_p = tool.Satuation(predict_point_angle*LCC_C_PIDP,-100.f,100.f);
    //积分量
    bool intergral_enable = 1;
    if(input.control_enable == 0 || input.stop_flag == 0){
        intergral_enable = 0;
    }
    float angle_pid_i = tool.Integrate(2, angle_pid_p*LCC_C_PIDI, -15, 15, 100, intergral_enable);
    internal_state.angle_feedback = angle_pid_i+angle_pid_p;
}

void Control::AngleFeedforward(){
    //c2的前馈,自行车模型
    float tmp = LCC_C_DisWheelbase/(0.5/tool.DivideProtect(input.c2));
    float feedforward_by_c2 = atan(tmp)*LCC_C_RatioGear*57.3;
    //roll的前馈，标定
    float feedforward_by_roll = (input.roll_angle + LCC_C_AngleRollError)*LCC_C_AngleRollGain;
    feedforward_by_roll = tool.Satuation(feedforward_by_roll, -20, 20);
    //前馈角度 = c2前馈 + roll前馈 + 方向盘对中误差
    float feedforward_angle = feedforward_by_c2 + feedforward_by_roll + LCC_C_AngleInitError;
    internal_state.angle_feedforward = feedforward_angle;
}

void Control::AngleOutDeal(){
    //微分
    float angle_sum = internal_state.angle_feedback + internal_state.angle_feedforward;
    float angle_derivative = tool.Derivative(1, angle_sum*LCC_C_PIDD, -20, 20, 100, input.control_enable);
    angle_sum += angle_derivative;
    //滤波限幅
    float angle_filter = tool.ButterworthFilter(2, angle_sum, LCC_C_FilterOutPut, 100);
    float angle_rate_limit = tool.RateLimit(1, angle_filter, LCC_C_AngleOutMaxRateLimit, 100);
    float angle_max_limit = tool.Satuation(angle_rate_limit, -1*LCC_C_AngleOutMaxLimit, LCC_C_AngleOutMaxLimit);
    //选择输出方向盘转角或前轮转角
    float angle_out = 0;
    if(LCC_C_FlagOutFrontWheelAngle == 1){
        angle_out = angle_max_limit/LCC_C_RatioGear;
    }
    else{
        angle_out = angle_max_limit;
    }
    internal_state.angle_out = angle_out;
}

void Control::SetInput(){
    // input = control_input
    std::string control_yaml_path = "/home/nvidia/control_cpp/control_cpp/src/control/include/control.yaml";
    std::ifstream fin(control_yaml_path);
    read_input = YAML::Load(fin);
    input.c0 = read_input["c0"].as<float>();
    input.c1 = read_input["c1"].as<float>();
    input.c2 = read_input["c2"].as<float>();
    input.control_enable = read_input["control_enable"].as<bool>();
    input.expect_accelaration = read_input["expect_accelaration"].as<float>();
    input.expect_distance = read_input["expect_distance"].as<float>();
    input.expect_velocity_kph = read_input["expect_velocity_kph"].as<float>();
    input.pitch_angle = read_input["pitch_angle"].as<float>();
    input.roll_angle = read_input["roll_angle"].as<float>();
    input.stop_flag = read_input["stop_flag"].as<bool>();
    input.vehicle_velocity_kph = read_input["vehicle_velocity_kph"].as<float>();
}

//每10ms调用一次该函数
ControlOutput Control::CalculateControl(){
    SetInput();
    TrajectoryToAccelaration();
    VelocitySolveAccelaration();
    ThrottleFeedForward();
    ThrottleFeedBack();
    AngleFeedback();
    AngleFeedforward();
    AngleOutDeal();
    BrakeControl();
    ControlOutput out;
    out.brake_enable_flag = internal_state.brake_enable_flag;
    out.brake_value = internal_state.brake_value;
    out.steer_angle = internal_state.angle_out;
    out.throttle = internal_state.throttle_final;
    return out;
}